#include "SkelMesh.h"
#include "DecoLogger.h"
#include "PrimitiveMesh.h"

DecoSkelMesh::DecoSkelMesh()
{
	mLocalToWorld.identity();
	mCOM = vector3(0, 0, 0);
}
DecoSkelMesh::~DecoSkelMesh()
{
	for (BodyGeomList::iterator it = mBodyGeoms.begin(); it != mBodyGeoms.end(); ++it)
	{
		delete *it;
	}
	mBodyGeoms.clear();

}

void DecoSkelMesh::SetBodyLinks(const std::vector<BodyLink*> links)
{
	mBodyLinks = links;
}

void DecoSkelMesh::ConstructFromModel(Model* model)
{
	mModel = model;
	matrix44 localToWorld;
	Vec3d radi;
	Mat4d transform;
	DecoStaticMesh* mesh = NULL;
	GeomEllipsoid* ellipse = NULL;
	const double rho_water = 1000.0;
	double inertia[3][3];
	double mass;
	int numBodyLinks = static_cast<int>(mModel->getNumBodyLinks());
	mBodyGeoms.resize(numBodyLinks);
	mBodyLinks.resize(numBodyLinks);
	mBodyTypes.resize(numBodyLinks);
	for (int i = 0; i < numBodyLinks; ++i)
	{
		BodyLink* b = mModel->getBodyLink(i);
		GeomObject* g = b->getPrimitive()->getGeom();
		mesh = new DecoStaticMesh();
		PrimitiveMesh* meshPrimitive = NULL;
		std::string meshFilePath, defaultPath = "../models/swim/";
		mBodyLinks[i] = b;
		mBodyGeoms[i] = mesh;
		switch (g->getType())
		{
		case GeomObject::ELLIPSOID : 
			mesh->ReadFromFile("../models/predefined/unitSphere.ASE");
			ellipse = static_cast<GeomEllipsoid*>(g);
			radi = ellipse->getRadii();
			mRadii = vector3(radi[0], radi[1], radi[2]);
			transform = ellipse->getWorldTransform();
			localToWorld = FromMat4dToMatrix44(transform);
			mesh->Scale(radi[0], radi[1], radi[2]); 
			mesh->SetLocalToWorld(localToWorld);
			mLocalCoord.push_back(localToWorld);
			mBodyTypes[i] = BGT_ELLIPSOID;
			break;
		case GeomObject::MESH:
			meshPrimitive = static_cast<PrimitiveMesh*>(b->getPrimitive());
			meshFilePath = meshPrimitive->getMeshPath();
			mesh->ReadFromObjFile(defaultPath + meshFilePath);
			ellipse = static_cast<GeomEllipsoid*>(g);
			radi = ellipse->getRadii();
			mRadii = vector3(radi[0], radi[1], radi[2]);
			mesh->ScaleTo(radi[0], radi[1], radi[2]);
			mesh->InitKdTree();
//			mesh->Scale(0.03, 0.03, 0.03);


			//b->getPrimitive()->setMass(mass);

//			b->getPrimitive()->setInertia(inertia);

			transform = ellipse->getWorldTransform();
			localToWorld = FromMat4dToMatrix44(transform);
			mesh->SetLocalToWorld(localToWorld);
			mLocalCoord.push_back(localToWorld);
			mBodyTypes[i] = BGT_MESH;

			//mass = calculateMassAndInertia(mesh, rho_water, inertia);
			break;
		default:
			(*(DecoLogger::GetSingleton())) << "Not yet implemented for geom other than ELLIPSOID" << __LINE__ << __FILE__ << "\n";
			break;
		}

	}
	CalculateBoudingBox();
}

void DecoSkelMesh::DumpToObjFile(const string& fileName)
{
	//ofstream out(fileName.c_str());
	//int numBodies = static_cast<int>(mBodyGeoms.size());
	//std::vector<vector3> allVertices;
	//for (int i = 0; i < numBodies; ++i)
	//{
	//	std::vector<vector3> allBodyVertices;
	//	mBodyGeoms[i]->RetrieveAllVertices(allBodyVertices);
	//	for (size_t j = 0; j < allBodyVertices.size(); ++j)
	//		allVertices.push_back(allBodyVertices[j]);
	//}
	//int numVertices = static_cast<int>(allVertices.size());
	//int numFaces = numVertices / 3;
	//for (int i = 0; i < numVertices; i++)
	//	out<<"v "<< allVertices[i].x <<" "<< allVertices[i].y <<" "<< allVertices[i].z << std::endl;
	//for (int i = 0; i < numFaces; i++)
	//	out<<"f "<< i * 3 + 1 <<" "<< i * 3 + 2<<" "<< i * 3 + 3 << std::endl;
	int numBodies = static_cast<int>(mBodyGeoms.size());

	for (int i = 0; i < numBodies; ++i)
	{
		string newFileName = fileName.substr(0, fileName.size() - 4);
		newFileName = newFileName + mBodyLinks[i]->getName() + ".obj";
		mBodyGeoms[i]->DumpToObjFile(newFileName);
	}

}

void DecoSkelMesh::CalculateBoudingBox()
{
	int numBodies = mModel->getNumBodyLinks();
	if (numBodies)
		mBoundingBox = mBodyGeoms[0]->GetBoundingBox();
	for (int i = 1; i < numBodies; ++i)
	{
		Box bb = mBodyGeoms[i]->GetBoundingBox();
		mBoundingBox += bb;
	}
}

Box DecoSkelMesh::GetBoundingBox()
{
	CalculateBoudingBox();
	return mBoundingBox;
}
Box	DecoSkelMesh::GetUntransformedBoundingBox() const
{
	return Box(vector3(MIN_DOUBLE, MIN_DOUBLE, MIN_DOUBLE), vector3(MAX_DOUBLE, MAX_DOUBLE, MAX_DOUBLE));
}
void DecoSkelMesh::SetLocalToWorld(const matrix44& matrix)
{
	mLocalToWorld = matrix;
}

void DecoSkelMesh::SetCOM(const vector3& com)
{
	mCOM = com;
}
BOOL DecoSkelMesh::NeedSort()
{
	return FALSE;
}
void DecoSkelMesh::Render(DecoRenderInterface* RI, DecoLight** Lights, INT numLights, DecoDrawType DrawType)
{
	RenderOpaqueSection(RI, Lights, numLights, DrawType);
}
void DecoSkelMesh::RenderTransparentSection(DecoRenderInterface* RI, DecoLight** Lights, INT numLights, DecoDrawType DrawType)
{

}
void DecoSkelMesh::RenderOpaqueSection(DecoRenderInterface* RI, DecoLight** Lights, INT numLights, DecoDrawType DrawType)
{
	//glPointSize(5);
	//glColor3f(1.0, 1.0, 1.0);
	//glBegin(GL_POINTS);
	//glVertex3d(mCOM.x, mCOM.y, mCOM.z);
	//glEnd();
	vector<BodyLink*> toVistList;

	toVistList.push_back(mModel->getRoot());

	while (!toVistList.empty())
	{
		BodyLink* currentBodyLink = toVistList[toVistList.size() - 1];
		toVistList.pop_back();
		int numChildLinks = currentBodyLink->getNumChildJoints();
		for (int i = 0; i < numChildLinks; ++i)
		{

			Joint* childJoint = currentBodyLink->getChildJoint(i);
			BodyLink* nextBody = childJoint->getBodyOut();
			if (nextBody)
			{
				toVistList.push_back(nextBody);
			}
		}

		Mat4d transformVL = vl_I;
		transformVL = currentBodyLink->getWorldTransform();
		matrix44 transform = FromMat4dToMatrix44(transformVL);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glRotatef(90.0f, 1, 0, 0);
		matrix44 mat;
		glGetDoublev(GL_MODELVIEW_MATRIX, (double*)&mat);
		glLoadIdentity();

		drawBodyLink(RI, Lights, numLights, DrawType, currentBodyLink, mat * mLocalToWorld * transform);
	}
	
}

bool DecoSkelMesh::InsideIthBody(int i, const vector3& pt, bool bUseVote)
{
	vector3 tmp;
	switch (mBodyTypes[i])
	{
	case BGT_MESH: 	case BGT_ELLIPSOID:
		return mBodyGeoms[i]->IsPointInside(pt, bUseVote);
		break;
	default:
		return false;
		break;
	}
}

BOOL DecoSkelMesh::IsPointInside(const vector3& pt, bool bUseVote)
{
	for (size_t i = 0; i < mBodyLinks.size(); ++i)
	{
		if (InsideIthBody(static_cast<int>(i), pt, bUseVote))
			return TRUE;
	}
	return FALSE;
}

DecoRenderData* DecoSkelMesh::GetRenderDataFromIthBody(int ithBody)
{
	DecoStaticMesh* mesh = mBodyGeoms[ithBody];
	BodyLink* b = mBodyLinks[ithBody];

	Mat4d transformVL = vl_I;
	transformVL = b->getWorldTransform();
	matrix44 transform = FromMat4dToMatrix44(transformVL);

	mesh->SetLocalToWorld(mLocalToWorld * transform * mLocalCoord[ithBody]);
	return mesh;
}

void DecoSkelMesh::drawBodyLink(DecoRenderInterface* RI, DecoLight** Lights, INT numLights, DecoDrawType DrawType, BodyLink* b, const matrix44& mat)
{
	BodyLinkList::iterator it = find(mBodyLinks.begin(), mBodyLinks.end(), b);
	assert(it != mBodyLinks.end());
	int ithBody = it - mBodyLinks.begin();
	BodyGeomList::iterator itGeom = mBodyGeoms.begin() + ithBody;
	DecoStaticMesh* mesh = *itGeom;

	//matrix44 originalTransform = mesh->GetLocalToWorld();
	//mesh->SetLocalToWorld(mat * originalTransform);
	//mesh->RenderOpaqueSection(RI, Lights, numLights, DrawType);
	//mesh->SetLocalToWorld(originalTransform);
	mesh->SetLocalToWorld(mat * mLocalCoord[ithBody]);
	mesh->RenderOpaqueSection(RI, Lights, numLights, DrawType);
}

BOOL DecoSkelMesh::RayIntersection(const vector3& origin, const vector3& dir, vector3& intersectionPt, DOUBLE& time, vector3* normal)
{
	return FALSE;
}


const matrix44& DecoSkelMesh::GetLocalToWorld() const
{
	return mLocalToWorld;
}

VOID DecoSkelMesh::Scale(DOUBLE xcoef, DOUBLE ycoef, DOUBLE zcoef) 
{

}
int DecoSkelMesh::RayIntersectionCount(const vector3& origin, const vector3& dir)
{
	return 0;
}
BOOL DecoSkelMesh::IsPrimitiveInBox(Box& box)
{
	return FALSE;
}
void DecoSkelMesh::RetrieveAllVertices(vector<vector3>& allVertices)
{

}
void DecoSkelMesh::RenderBoundingBox(DecoRenderInterface* RI)
{

}

#define NUM_SAMPLES_PER_DIM 30
#define NUM_SAMPLES_MASS_ESTIMATE (NUM_SAMPLES_PER_DIM*NUM_SAMPLES_PER_DIM*NUM_SAMPLES_PER_DIM)

double DecoSkelMesh::calculateMassAndInertia(DecoStaticMesh* mesh, double rho_water, double inertia[3][3])
{
	vector<vector3> hitSamples;
	const Box& bb = mesh->GetBoundingBox();
	vector3 llb = bb.GetMin();
	vector3 urf = bb.GetMax();
	vector3 stepSize = (urf - llb) / NUM_SAMPLES_PER_DIM; 
	for (int i = 0; i < NUM_SAMPLES_PER_DIM; ++i)
		for (int j = 0; j < NUM_SAMPLES_PER_DIM; ++j)
			for (int k = 0; k < NUM_SAMPLES_PER_DIM; ++k)
			{
				vector3 sample((i + 0.5) * stepSize.x + llb.x, (j + 0.5) * stepSize.y + llb.y, (k + 0.5) * stepSize.z + llb.z);
				//vector3 sample(RandDouble(llb.x, urf.x), RandDouble(llb.y, urf.y), RandDouble(llb.z, urf.z));
				if (mesh->IsPointInside(sample))
				{
					hitSamples.push_back(sample);
				}
			}

	double bbVolume = (urf.x - llb.x) * (urf.y - llb.y) * (urf.z - llb.z);
	int num = static_cast<int>(hitSamples.size());
	double mass = rho_water * (static_cast<double>(num) / NUM_SAMPLES_MASS_ESTIMATE) * bbVolume;

	//assume the center of mass is (0, 0, 0)
	double sampleMass = mass / num;
	for (int i = 0; i < 3; ++i)
		for (int j = 0; j < 3; ++j)
			inertia[i][j] = 0;
	vector3 com(0, 0, 0);
	for (int i = 0; i < num; ++i)
	{
		vector3 sample = hitSamples[i];
		inertia[0][0] += sampleMass * (sample.y * sample.y + sample.z * sample.z);
		inertia[1][1] += sampleMass * (sample.x * sample.x + sample.z * sample.z);
		inertia[2][2] += sampleMass * (sample.y * sample.y + sample.x * sample.x);
		inertia[0][1] += -sampleMass * (sample.x * sample.y);
		inertia[0][2] += -sampleMass * (sample.x * sample.z);
		inertia[1][2] += -sampleMass * (sample.y * sample.z);
		inertia[1][0] += -sampleMass * (sample.x * sample.y);
		inertia[2][0] += -sampleMass * (sample.x * sample.z);
		inertia[2][1] += -sampleMass * (sample.y * sample.z);
		com += sample;
	}
	com /= num;
	return mass;
}

